Velocity Command Ignored Checking #321

cli18 · 2023-07-13T02:27:50Z

Problem Summary:
When the robot is disabled/not moving due to hardware/firmware issues or obstacles, and the planner keeps feeding non-zero velocity commands to the robot, we need to inform the users know something is wrong with the robot that it is ignoring cmd_vel.

Feature:
This is an implementation of the velocity ignored checking and error displaying feature. When the robot is not moving for N seconds(set by a parameter) while the planner keeps publishing non-zero commands, it will output an error and fatal message on the ROS console and sends a error code. Velocity command ignored check is performed every time we got an new velocity command from the planner.

If user would like to enable the feature, change the cmd_vel_ignored_tolerance parameter to be higher than zero. Otherwise, set a value below zero.

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

mbf_abstract_nav/src/abstract_controller_execution.cpp

…controller state

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

mbf_abstract_nav/src/abstract_controller_execution.cpp

mbf_abstract_nav/src/controller_action.cpp

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

corot · 2023-07-18T23:23:45Z

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

+     * @param cmd_velocity the latest cmd_vel being published to the robot
+     * @return true if cmd_vel is being ignored by the robot longer than tolerance time, false otherwise
+     */
+    bool checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity);


S: a bit more precise; and cmd_vel is the conventional name we use

Suggested change

bool checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity);

bool checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel);

mbf_abstract_nav/src/abstract_controller_execution.cpp

mbf_abstract_nav/src/controller_action.cpp

corot · 2023-07-18T23:36:38Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+          // check if robot ignores velocity command
+          if (robot_ignore_vel_enabled && checkVelocityIgnore(cmd_vel_stamped.twist))


Suggested change

// check if robot ignores velocity command

if (robot_ignore_vel_enabled && checkVelocityIgnore(cmd_vel_stamped.twist))

// check if robot is ignoring velocity commands

if (robot_ignore_vel_enabled && checkVelocityIgnore(cmd_vel_stamped.twist))

corot · 2023-07-18T23:36:57Z

mbf_abstract_nav/src/abstract_controller_execution.cpp


+  // check if the velocity ignore check is enabled or not
+  bool robot_ignore_vel_enabled = (robot_ignore_check_tolerance_ > 0.0);


S: don't need ()

corot · 2023-07-18T23:38:10Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+  const double cmd_linear = std::hypot(cmd_velocity.linear.x, cmd_velocity.linear.y);
+  const double cmd_angular = std::abs(cmd_velocity.angular.z);
+
+  bool cmd_is_not_zero = (cmd_linear > 0.01 || cmd_angular > 0.01);


S: this is forcing u to make a double negation on the if below (avoid contorted logic whenever possible)

corot · 2023-07-18T23:38:57Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+  // check if robot ignores the cmd_vel
+  double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
+  ROS_WARN_THROTTLE(1,
+                    "Robot ignores velocity command for %.2f seconds.\n The cmd_vel being ignored is: (x=%.2f, "


Suggested change

"Robot ignores velocity command for %.2f seconds.\n The cmd_vel being ignored is: (x=%.2f, "

"Robot ignores velocity command for %.2f seconds (commanded velocity: x=%.2f, "

corot · 2023-07-18T23:40:21Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+  if (ignored_duration > robot_ignore_check_tolerance_)
+  {
+    // the robot is ignoring the velocity command more the threshold time
+    ROS_ERROR("Robot is disabled and the time it ignored velocity command exceeded the tolerance time: %.2f seconds",


F: we don't know why is ignoring vel commands, so better not to make statements like "robot is disabled" can be off, can be blocked against a wall,,,, we don't know

corot

much better, but still needs some work

general comment: I think here "is ignoring" makes more sense than "ignores" (but u live in USA, so i trust your criteria)

mbf_abstract_nav/src/abstract_controller_execution.cpp

mbf_abstract_nav/src/controller_action.cpp

corot · 2023-07-21T01:48:23Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+  const double cmd_linear = std::hypot(cmd_vel.linear.x, cmd_vel.linear.y);
+  const double cmd_angular = std::abs(cmd_vel.angular.z);
+
+  const bool cmd_is_zero = (cmd_linear < 0.01 && cmd_angular < 0.01);


S: maybe use 1e-3 here too?
S: I think u can remove the brackets (not 100% sure)

I tested without the brackets and it was working fine. So I removed it.

corot · 2023-07-21T01:50:04Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+    // the robot is ignoring the velocity command more the threshold time
+    ROS_ERROR("Robot is ignoring velocity command for more than the tolerance time: %.2f seconds",
+              cmd_vel_ignored_tolerance_);
+    return true;  // return true


Suggested change

return true; // return true

return true;

corot · 2023-07-21T01:53:25Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+

extra line?

corot · 2023-07-24T08:17:06Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

@@ -185,6 +186,63 @@ void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamp
  // TODO so there should be no loss of information in the feedback stream
 }

+bool AbstractControllerExecution::checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel)
+{
+  // check if the velocity ignore check is enabled or not


Suggested change

// check if the velocity ignore check is enabled or not

// check if the velocity ignored check is enabled or not

corot · 2023-07-24T08:17:34Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+    // set to zero if not already zero
+    if (!first_ignored_time_.is_zero())
+    {
+      first_ignored_time_ = ros::Time();
+    }


F: not needed if we are not checking vel ignored

corot · 2023-07-24T08:20:01Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+  // velocity is not being ignored
+  if (!robot_stopped || cmd_is_zero)
+  {
+    // set to zero if not already zero


F: this comment says the same as the code. Write comments only to clarify non-obvious steps

same goes for the next 2 comments

corot · 2023-07-24T08:21:01Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+    first_ignored_time_ = ros::Time::now();
+  }
+
+  // check if robot ignores the cmd_vel


F: comment is wrong. you already know that the robot is ignoring

corot · 2023-07-24T08:23:39Z

mbf_abstract_nav/src/controller_action.cpp

@@ -310,6 +310,14 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
        goal_handle.setAborted(result, result.message);
        break;

+      case AbstractControllerExecution::ROBOT_DISABLED:
+        ROS_FATAL_STREAM_NAMED(name_, "Robot ignored velocity commands for more than tolerance time");


F: You will have almost the same error log twice

corot

almost there...

corot · 2023-07-25T04:50:03Z

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

@@ -234,6 +242,9 @@ namespace mbf_abstract_nav
    //! The time the controller responded with a success output (output < 10).
    ros::Time last_valid_cmd_time_;

+    //! The first time when the robot is ignoring velocity command


Suggested change

//! The first time when the robot is ignoring velocity command

//! The time when the robot started ignoring velocity commands

mbf_abstract_nav/src/abstract_controller_execution.cpp

corot

good!

I added some extra suggestion

corot · 2023-07-25T04:52:30Z

mbf_abstract_nav/src/abstract_controller_execution.cpp

+  }
+
+  const double ignored_duration = (ros::Time::now() - first_ignored_time_).toSec();
+  ROS_WARN_THROTTLE(1,


S: maybe this should go after the error (line 234) so you don't show both WARN and ERROR together

mbf_abstract_nav/src/abstract_controller_execution.cpp

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

created velocity command check feature

cb5f286

siferati requested review from corot and siferati July 13, 2023 03:27

siferati marked this pull request as ready for review July 13, 2023 03:29

fixed formatting

924c598

corot reviewed Jul 13, 2023

View reviewed changes

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h Outdated Show resolved Hide resolved

corot reviewed Jul 13, 2023

View reviewed changes

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h Outdated Show resolved Hide resolved

siferati reviewed Jul 13, 2023

View reviewed changes

mbf_abstract_nav/src/abstract_controller_execution.cpp Outdated Show resolved Hide resolved

fixed velocity check logic and variable naming, added ROBOT_DISABLED …

21d738c

…controller state

siferati reviewed Jul 17, 2023

View reviewed changes

Fix: address Tiago's comments

589e118

cli18 requested review from siferati and corot July 18, 2023 04:14

corot reviewed Jul 18, 2023

View reviewed changes

mbf_abstract_nav/src/abstract_controller_execution.cpp Outdated Show resolved Hide resolved

corot reviewed Jul 18, 2023

View reviewed changes

mbf_abstract_nav/src/abstract_controller_execution.cpp Outdated Show resolved Hide resolved

corot reviewed Jul 18, 2023

View reviewed changes

mbf_abstract_nav/src/controller_action.cpp Outdated Show resolved Hide resolved

corot reviewed Jul 18, 2023

View reviewed changes

corot requested changes Jul 18, 2023

View reviewed changes

siferati reviewed Jul 18, 2023

View reviewed changes

fixed phrasing and logic

bb6eb9a

cli18 requested review from corot and siferati July 19, 2023 02:52

edited commenting

87f7c05

cli18 changed the title ~~Velocity Command Checking for Disabled Robot~~ Velocity Command Ignored Checking Jul 20, 2023

corot reviewed Jul 21, 2023

View reviewed changes

mbf_abstract_nav/src/abstract_controller_execution.cpp Outdated

Copy link

Collaborator

corot Jul 21, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

extra line?

addressed Jorge's comments

5517f16

cli18 requested a review from corot July 24, 2023 01:02

corot reviewed Jul 24, 2023

View reviewed changes

corot requested changes Jul 24, 2023

View reviewed changes

addressed additional comments from Jorge

caed98a

cli18 requested a review from corot July 25, 2023 01:15

corot reviewed Jul 25, 2023

View reviewed changes

mbf_abstract_nav/src/abstract_controller_execution.cpp Outdated Show resolved Hide resolved

corot approved these changes Jul 25, 2023

View reviewed changes

addressed Jorge's extra suggestions

faa8654

siferati reviewed Jul 26, 2023

View reviewed changes

siferati approved these changes Jul 27, 2023

View reviewed changes

addressed Tiago's suggestions

e566020

corot merged commit ac5adb4 into naturerobots:master Jul 31, 2023
3 of 5 checks passed

cli18 deleted the cl/vel_checking branch August 1, 2023 00:40

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Velocity Command Ignored Checking #321

Velocity Command Ignored Checking #321

cli18 commented Jul 13, 2023 •

edited

Loading

corot Jul 18, 2023 •

edited

Loading

corot Jul 18, 2023

corot Jul 18, 2023

corot Jul 18, 2023

corot Jul 18, 2023

corot Jul 18, 2023

corot left a comment

corot Jul 21, 2023

cli18 Jul 24, 2023

corot Jul 21, 2023

corot Jul 21, 2023

corot Jul 24, 2023

corot Jul 24, 2023

corot Jul 24, 2023

corot Jul 24, 2023

corot Jul 24, 2023

corot Jul 24, 2023

corot left a comment

corot Jul 25, 2023

corot left a comment

corot Jul 25, 2023

	bool checkVelocityIgnore(const geometry_msgs::Twist& cmd_velocity);
	bool checkCmdVelIgnored(const geometry_msgs::Twist& cmd_vel);

		// check if robot ignores velocity command
		if (robot_ignore_vel_enabled && checkVelocityIgnore(cmd_vel_stamped.twist))


		// check if the velocity ignore check is enabled or not
		bool robot_ignore_vel_enabled = (robot_ignore_check_tolerance_ > 0.0);

	"Robot ignores velocity command for %.2f seconds.\n The cmd_vel being ignored is: (x=%.2f, "
	"Robot ignores velocity command for %.2f seconds (commanded velocity: x=%.2f, "

	// check if the velocity ignore check is enabled or not
	// check if the velocity ignored check is enabled or not

	//! The first time when the robot is ignoring velocity command
	//! The time when the robot started ignoring velocity commands

Velocity Command Ignored Checking #321

Velocity Command Ignored Checking #321

Conversation

cli18 commented Jul 13, 2023 • edited Loading

corot Jul 18, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

corot left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

corot left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

corot left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

cli18 commented Jul 13, 2023 •

edited

Loading

corot Jul 18, 2023 •

edited

Loading